/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include"PoseMCL.h"
#include <sys/time.h>
#include <time.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>

float timer(bool isStart){
	static long int ss1,us1;
	long int ss2,us2,sec,us,ms;
	float fms;
	struct timeval tv;
	 
	
	if(isStart){
		gettimeofday(&tv, NULL);
		ss1 = tv.tv_sec;
		us1 = tv.tv_usec;
	}
	else{
		gettimeofday(&tv, NULL);
		ss2 = tv.tv_sec;
		us2 = tv.tv_usec;
		sec = ss2-ss1;
		us = us2-us1;
		ms = sec*1000+us/1000;
		fms=(float)sec*1000.0+(float)us/1000.0;
		//fprintf(stderr,"Time elapsed (%.6fms = %.6fs) \n",fms,fms/1000.0);
		if(!isStart) return fms;
		else return 0;
	}
}


int main(){	
	CPoseMCL pmcl;
	ownRandom myrand; 
	
	pmcl.initialize(500, mcl::pose(0,0,0),	mcl::pose(0.5,0.5,M_PI/4));
	mcl::pose gt;
	while(1){
		timer(true);
		pmcl.predict(mcl::pose(0.05,0,0), mcl::pose(0.01,0.001,M_PI/180));
		gt.x+=0.05; ///Ground truth
		///posemeas
		mcl::pose meas(gt.x+myrand.normalRandom()*0.05,myrand.normalRandom()*0.05,myrand.normalRandom()*3.0*M_PI/180.0);
		pmcl.update(meas);
		
		mcl::pose fpose = pmcl.filt.getDistributionMean(true);
		float fms = timer(false);
		fprintf(stderr,"%.3fms true (%.2f %.2f %.2f) -- filt (%.2f %.2f %.2f) \n",fms,gt.x,gt.y,gt.a, fpose.x,fpose.y,fpose.a);
		//usleep(1000*200);
	}
	
	
	return 0;
}